#include "pop_ball.h"
#include "FreeRTOS.h"
#include "task.h"

static void pwm_set(uint16_t p1,uint16_t p2,uint16_t b1,uint16_t b2);
static void gpio_set(uint16_t pin,GPIO_PinState st);

/**
 * @brief 	初始化
 */
void pop_ball_init()
{
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
	
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,1000);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_2,1000);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_3,1000);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,1000);
	
	close_pump();
}

/**
 * @brief 	改tim4上PWM的脉宽		
 * @param 	p1,p2		油门线脉宽		@arg 1000~2000
 * @param	b1,b2		刹车线脉宽		@arg 1000~2000
 */
static void pwm_set(uint16_t p1,uint16_t p2,uint16_t b1,uint16_t b2)
{
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,p1);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_2,p2);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_3,b1);
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,b2);
}

/**
 * @brief 	写GPIOC上的引脚		
 * @param 	pin			引脚	@arg  Px ,(x = 0,1,2,3)
 * @param	st			电平	@arg  1 或者 0
 */
static void gpio_set(uint16_t pin,GPIO_PinState st)
{
	HAL_GPIO_WritePin(GPIOC,pin,st);
}

/**
 * @brief 	设置两个5065		
 * @param 	st		两个电机运行状态		@arg 	1 (正转)，2(反转)，3(停止)
 */
void motor_5065_key(MOTOR_5065_STATE st)
{
	uint16_t pedal = 1200;
	uint16_t pedal_i = 1600;
	switch(st)
	{
		case 1:
		{
			pwm_set(pedal,pedal,1000,1000);
			break;
		}
		case 2:
		{
			pwm_set(pedal_i,pedal_i,2000,2000);
			break;
		}
		case 3:
		{
			pwm_set(1000,1000,1000,1000);
			break;
		}
	}
}

void open_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_0,1);   //爪子开
    HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,0);   //爪子关
	vTaskDelay(pdMS_TO_TICKS(100));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_0,0);   //爪子开
    HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,0);   //爪子关
	
}

void close_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_0,0);   //爪子开
    HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,1);   //爪子关
	vTaskDelay(pdMS_TO_TICKS(100));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_0,0);   //爪子开
    HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,0);   //爪子关
}

void down_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,0);   //拍球上
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_2,1);   //拍球下
	vTaskDelay(pdMS_TO_TICKS(40));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_2,0);   //拍球下
}
void up_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,1);   //拍球上
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_2,0);   //拍球下
	vTaskDelay(pdMS_TO_TICKS(80));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,0);   //拍球上
}



void go_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_1,1);   //前
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,0);   //后
	vTaskDelay(pdMS_TO_TICKS(100));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_1,0);   //前
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,0);   //后
	
}

void back_pump(void)
{
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_1,0);   //前
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,1);   //后
	vTaskDelay(pdMS_TO_TICKS(100));
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_1,0);   //前
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,0);   //后
	
}





